#include "motor.h"
#include "ros/ros.h"

#define motor_num							(18)
#define motor_angle_lpf_kp		(0.1f)

float motor_angles[motor_num];
int16_t angle_lpf[motor_num];

/**
 * @brief  控制电机
 * @param  
 * @retval 无
 */
void motor_set_angle_comm(uint8_t id, int16_t *angle_buff, uint8_t num)
{
	uint8_t tx_buff[42];
	
	tx_buff[0] = id;
	tx_buff[1] = num;
	
	for (uint8_t i=0; i < num; i++) {
		tx_buff[2 + i*2 + 0] = (angle_buff[i]>>8);
		tx_buff[2 + i*2 + 1] = angle_buff[i] & 0x00ff;
	}
	
	comm_tx(motor_angle, tx_buff, num*2 + 2);

	ROS_INFO("%d %d %d", angle_buff[0], angle_buff[1], angle_buff[2]);
}

/**
 * @brief  设置单个舵机的角度
 * @param  
 * @retval 无
 */
void motor_set_angle_single_comm(uint8_t id, int16_t angle)
{
	motor_set_angle_comm(id, &angle, 1);
}

/**
 * @brief  电机控制任务
 * @param  
 * @retval 无
 */
void motor_ctrl_task(void)
{
	for (uint8_t i=0; i < motor_num; i++) {
		angle_lpf[i] = motor_angles[i];
	}
	
	motor_set_angle_comm(1, angle_lpf, 18);
}

/**
 * @brief  设置角度
 * @param  
 * @retval 无
 */
void motor_set_angle_single(uint8_t num, float angle)
{
	if (num > motor_num) {
		return;
	}
	
	motor_angles[num] = angle;
}

